﻿using System;
using System.Collections.Generic;
using FarseerPhysics.Dynamics;
using Microsoft.Xna.Framework;

namespace FarseerPhysics.Controllers
{
    /// <summary>
    /// Put a limit on the linear (translation - the movespeed) and angular (rotation) velocity
    /// of bodies added to this controller.
    /// </summary>
    public class VelocityLimitController : Controller
    {
        //public bool LimitAngularVelocity = true;
        public bool LimitLinearVelocity = true;
        private List<Body> _bodies = new List<Body>();
        private float _maxLinearSqaredX;
        private float _maxLinearSqaredY;
        private float _maxLinearVelocityX;
        private float _maxLinearVelocityY;

        /// <summary>
        /// Initializes a new instance of the <see cref="VelocityLimitController"/> class.
        /// Sets the max linear velocity to Settings.MaxTranslation
        /// Sets the max angular velocity to Settings.MaxRotation
        /// </summary>
        public VelocityLimitController()
            : base(ControllerType.VelocityLimitController)
        {
            MaxLinearVelocityX = Settings.MaxTranslation;
            MaxLinearVelocityY = Settings.MaxTranslation;
        }

        /// <summary>
        /// Initializes a new instance of the <see cref="VelocityLimitController"/> class.
        /// Pass in 0 or float.MaxValue to disable the limit.
        /// maxAngularVelocity = 0 will disable the angular velocity limit.
        /// </summary>
        /// <param name="maxLinearVelocity">The max linear velocity.</param>
        /// <param name="maxAngularVelocity">The max angular velocity.</param>
        public VelocityLimitController(float maxLinearVelocityX, float maxLinearVelocityY)
            : base(ControllerType.VelocityLimitController)
        {
            MaxLinearVelocityX = maxLinearVelocityX;
            MaxLinearVelocityY = maxLinearVelocityY;
        }

        /// <summary>
        /// Gets or sets the max linear velocity.
        /// </summary>
        /// <value>The max linear velocity.</value>
        public float MaxLinearVelocityX
        {
            get { return _maxLinearVelocityX; }
            set
            {
                _maxLinearVelocityX = value;
                _maxLinearSqaredX = _maxLinearVelocityX * _maxLinearVelocityX;
            }
        }

        public float MaxLinearVelocityY
        {
            get { return _maxLinearVelocityY; }
            set
            {
                _maxLinearVelocityY = value;
                _maxLinearSqaredY = _maxLinearVelocityY * _maxLinearVelocityY;
            }
        }

        public override void Update(float dt)
        {
            foreach (Body body in _bodies)
            {
                if (!IsActiveOn(body))
                    continue;

                if (LimitLinearVelocity)
                {
                    float velX = body.LinearVelocityInternal.X;
                    float velY = body.LinearVelocityInternal.Y;

                    //float result = velX * velX + velY * velY;
                    if (Math.Abs(body.LinearVelocityInternal.X) > MaxLinearVelocityX)
                    {
                        body.LinearVelocityInternal.X = MaxLinearVelocityX * Math.Sign(body.LinearVelocityInternal.X);
                    }

                    if (Math.Abs(body.LinearVelocityInternal.Y) > MaxLinearVelocityY)
                    {
                        body.LinearVelocityInternal.Y = MaxLinearVelocityY * Math.Sign(body.LinearVelocityInternal.Y);
                    }
                }
            }
        }

        public void AddBody(Body body)
        {
            _bodies.Add(body);
        }

        public void RemoveBody(Body body)
        {
            _bodies.Remove(body);
        }
    }
}